#include <ros/ros.h>
#include <std_msgs/String.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/Twist.h>

ros::Publisher vel_pub;
static int nCount = 0;

void LidarCallback(const sensor_msgs::LaserScan msg)
{
    int size;
    float dist;
    geometry_msgs::Twist vel_cmd;

    size = msg.ranges.size();
    dist = msg.ranges[size / 2];
    ROS_INFO("distance = %f", dist); 

    if(dist < 1.5f)
    {
        vel_cmd.angular.z = 0.3;
        vel_cmd.linear.x = 0;
        nCount = 50;
    }
    else
    {
        vel_cmd.linear.x = 0.2;
        vel_cmd.angular.z = 0;
    }
    vel_pub.publish(vel_cmd);
}

int main(int argc, char** argv)
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"demo_lidar_behavior");
    
    ros::NodeHandle n;
    ros::Subscriber lidar_sub = n.subscribe("/scan", 10, &LidarCallback);
    vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);

    ros::spin();
}
